#include "hailo_sys.h"



#define INFER_FRAME_COUNT (100)
#define MAX_EDGE_LAYERS (16)


char  cAppPath[512] = {0};
hailo_vdevice vdevice = 0;
hailo_hef hef = 0;
hailo_configure_params_t config_params ;
hailo_configured_network_group network_group = 0;
size_t   network_group_size = 1;
hailo_input_vstream_params_by_name_t input_vstream_params[MAX_EDGE_LAYERS];
hailo_output_vstream_params_by_name_t output_vstream_params[MAX_EDGE_LAYERS];
size_t   input_vstreams_size = MAX_EDGE_LAYERS;
size_t   output_vstreams_size = MAX_EDGE_LAYERS;
hailo_input_vstream input_vstreams[MAX_EDGE_LAYERS] = {0};
hailo_output_vstream output_vstreams[MAX_EDGE_LAYERS] = {0};

void* write_to_device(void *args)
{
    char cInputImg[512] = {0};
    hailo_status status = HAILO_UNINITIALIZED;
    size_t   input_frame_size = 0;
    uint8_t *src_data = 0;
    hailo_input_vstream vstream = *(hailo_input_vstream*)args;

    status = hailo_get_input_vstream_frame_size(vstream, &input_frame_size);


    src_data = (uint8_t *)malloc(input_frame_size);

    sprintf(cInputImg,"%s/half.png",cAppPath);

    cv::Mat   imgGray;
    cv::Mat   imgRaw = cv::imread(cInputImg);
    cv::cvtColor(imgRaw,imgGray,cv::COLOR_BGR2GRAY);

    for(int i=0; i<imgGray.rows*imgGray.cols; i++)
    {
       src_data[i] = imgGray.at<uchar>(i);
    }

   // for (size_t i = 0; i < input_frame_size; i++) {
   //     src_data[i] = (uint8_t)(rand() % 256);
   // }

    for (uint32_t i = 0; i < INFER_FRAME_COUNT; i++) {
        status = hailo_vstream_write_raw_buffer(vstream, src_data, input_frame_size);
    }

    // Flushing is not mandatory here
    status = hailo_flush_input_vstream(vstream);
    if(status != HAILO_SUCCESS)
    {
        printf("hailo write input error \n");
    }

    status = HAILO_SUCCESS;
    if(src_data)
        free(src_data);
    return  0;
}

void * read_from_device(void *args)
{
    hailo_status status = HAILO_UNINITIALIZED;
    size_t   output_frame_size = 0;
    uint8_t *dst_data = 0;
    hailo_output_vstream vstream = *(hailo_output_vstream*)args;

    status = hailo_get_output_vstream_frame_size(vstream, &output_frame_size);
    if(status != HAILO_SUCCESS)
    {
        printf("hailo_get_output_vstream_frame_size read output  error \n");
    }

    dst_data = (uint8_t*)malloc(output_frame_size);



    int isWrite = 0;
    for (uint32_t i = 0; i < INFER_FRAME_COUNT; i++) {
        status = hailo_vstream_read_raw_buffer(vstream, dst_data, output_frame_size);

        if(isWrite == 0)
        {
            cv::Mat  mat32f = cv::Mat(312,480,CV_32FC1,dst_data);
            cv::Mat  dst8BitGray;

            mat32f.convertTo(dst8BitGray,CV_8U,255.0);

            cv::imwrite("./dstimg.png",dst8BitGray);

            //FILE*  frawfile = fopen("write_312_480.raw","wb");
            //fwrite(dst_data,sizeof(char),output_frame_size,frawfile);
            //if(frawfile)
            //  fclose(frawfile);
            isWrite = 1;
        }

    }

    status = HAILO_SUCCESS;

    if(dst_data)
        free(dst_data);

    return 0;
}

void hailo_init(const char* pAppPath)
{
    char cHefFile[512] = {0};
    hailo_status status = HAILO_UNINITIALIZED;


    bool unused = {0};

    status = hailo_create_vdevice(0, &vdevice);

    sprintf(cHefFile,"%s/espcn_x2.hef",pAppPath);

    status = hailo_create_hef_file(&hef, cHefFile);


    status = hailo_init_configure_params_by_vdevice(hef, vdevice, &config_params);
    if(status != HAILO_SUCCESS)
    {
        printf("inti vdevice\n");
    }

    status = hailo_configure_vdevice(vdevice, hef, &config_params, &network_group, &network_group_size);



    // Set input format type to auto - libhailort will not scale the data before writing to the HW
    status = hailo_make_input_vstream_params(network_group, unused, HAILO_FORMAT_TYPE_AUTO,
        input_vstream_params, &input_vstreams_size);


    /* The input format order in the example HEF is NHWC in the user-side (may be seen using 'hailortcli parse-hef <HEF_PATH>).
       Here we override the user-side format order to be NCHW */
    for (size_t  i = 0 ; i < input_vstreams_size; i++) {
        input_vstream_params[i].params.user_buffer_format.order = HAILO_FORMAT_ORDER_NCHW;
    }

    // Set output format type to float32 - libhailort will de-quantize the data after reading from the HW
    // Note: this process might affect the overall performance
    status = hailo_make_output_vstream_params(network_group, unused, HAILO_FORMAT_TYPE_FLOAT32,
        output_vstream_params, &output_vstreams_size);


    status = hailo_create_input_vstreams(network_group, input_vstream_params, input_vstreams_size, input_vstreams);

    status = hailo_create_output_vstreams(network_group, output_vstream_params, output_vstreams_size, output_vstreams);

    strncpy(cAppPath,pAppPath,512);
}



int  infer(hailo_input_vstream *input_vstreams, int input_vstreams_size,
    hailo_output_vstream *output_vstreams, int output_vstreams_size)
{
    int    status = HAILO_UNINITIALIZED;
    pthread_t   write_threads[MAX_EDGE_LAYERS] = {};
    pthread_t   read_threads[MAX_EDGE_LAYERS] = {};
    int   write_thread_status = 0;
    int   read_thread_status =  0;
    int     input_threads_index = 0;
    int     output_threads_index = 0;
    int     index = 0;

    // Create reading threads
    for (output_threads_index = 0; output_threads_index < output_vstreams_size; output_threads_index++) {
        status = pthread_create( &read_threads[output_threads_index] ,0,read_from_device,&output_vstreams[output_threads_index]);
    }

    // Create writing threads
    for (input_threads_index = 0; input_threads_index < input_vstreams_size; input_threads_index++) {
        status = pthread_create(&write_threads[input_threads_index],0, write_to_device,&input_vstreams[input_threads_index]);
    }

    // Join writing threads
    for (index = 0; index < input_threads_index; index++) {
        write_thread_status = pthread_join(write_threads[index],0);
        if (HAILO_SUCCESS != write_thread_status) {
            printf("write_thread failed \n");
            status = write_thread_status; // Override write status
        }
    }

    // Join reading threads
    for (index = 0; index < output_threads_index; index++) {
        read_thread_status = pthread_join(read_threads[index],0);
        if (HAILO_SUCCESS != read_thread_status) {
            printf("read_thread failed \n");
            status = read_thread_status; // Override read status
        }
    }

    return status;
}



void hailo_infer(const char* pAppPath)
{
    int  status =  0;
    status = infer(input_vstreams, input_vstreams_size, output_vstreams, output_vstreams_size);
    if(status == 0)
         printf("Inference ran successfully\n");
}



void hailo_exit()
{

    if(output_vstreams)
        (void)hailo_release_output_vstreams(output_vstreams, output_vstreams_size);

     if(input_vstreams)
        (void)hailo_release_input_vstreams(input_vstreams, input_vstreams_size);

     if(hef)
         (void) hailo_release_hef(hef);

     if(vdevice)
         (void) hailo_release_vdevice(vdevice);

}
